#include "message.h"
#include "motion.h"
#include "drive/Stream.h"
#include "mode_control.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "./drive/tw80st.h"

#define MSG_PORT SOURCE_USART1

TW80ST tw;

uint8_t cmd_line[CMD_BUF_SIZE];
uint16_t cmd_index = 0;

enum {
    MSG_OK,
    MSG_NONE,
    MSG_FAIL
};

void MessageInit() {
    SOURCE_USART_PARAM_T param = {9600, U_REMAP_NO};
    StreamInit(MSG_PORT, &param);
    cmd_index = 0;
}

uint16_t MessageGet() {
    uint8_t ch;
    if (StreamReadC(MSG_PORT, &ch) == STREAM_SUCCESS) {
        if (cmd_index == 0) {
            if (ch == '<') {
                cmd_line[cmd_index++] = ch;
            }
        } else {
            cmd_line[cmd_index++] = ch;
            if (ch == '>') {
                cmd_line[cmd_index] = 0;
                cmd_index = 0;
                return MSG_OK;
            }

        }
        if (cmd_index == CMD_BUF_SIZE) {
            cmd_index = 0;
            return MSG_FAIL;
        }
    }
    return MSG_NONE;
}

float MessageGetParam(uint8_t cmd) {
    uint8_t * p = cmd_line;
    while ((*p) != 0) {
        if ((*p++) == cmd) {
            return atof((const char*)p);
        }
    }
    return 0;
}

static uint8_t buf[30];
void ReportData() {
  sprintf((char *)buf, "<temp %ld>", tw.temperature_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<pm25 %lu>", tw.pm25_val_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<humd %lu>", tw.humdity_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<co2 %lu>", tw.co2_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<ch2o %lu>", tw.ch2o_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<tvoc %lu>", tw.tvoc_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
  sprintf((char *)buf, "<pm10 %lu>", tw.pm10_);
  StreamWrites(SOURCE_USART1, (uint8_t *)buf, strlen((char *)buf));
}

#define CMD_MOTOR 'M'
#define CMD_MOTOR_ANGLE 'D'
#define CMD_MOTOR_SPEED 'S'

#define CMD_FACE_TEST 'F'
#define CMD_AVOID 'A'
#define CMD_STATIC_MODE 'D'
#define CMD_GET_DATA 'H'

void MessagePares() {
    uint8_t cmd_type = cmd_line[1];
    if (cmd_type == CMD_MOTOR) {
        ModeSet(MODE_RREMOTE);
        int16_t angle = MessageGetParam(CMD_MOTOR_ANGLE);  // 角度0-359
        int16_t speed = MessageGetParam(CMD_MOTOR_SPEED);  // 运动速度 0-100
        MotionControl(angle, speed);
    } else if (cmd_type == CMD_AVOID){
      ModeSet(MODE_AVOID);
    } else if (cmd_type == CMD_STATIC_MODE){
      ModeSet(MODE_STATIC);
    } else if (cmd_type == CMD_FACE_TEST){
      ModeSet(MODE_FACE_TEST);
    } else if (cmd_type == CMD_GET_DATA) {
      ReportData();
    }
}

void MessageProcess() {
    if (MessageGet() == MSG_OK) {
        MessagePares();
    }
    tw.Loop();
}